#include "slepbase.h"
#include "epph.h"


void mcLeastR(slep_matrix* A, slep_matrix* Y, double z, OPTS* opts,
			  slep_matrix* X, slep_vector* funVal, slep_vector* valueL){

	double q, lambda , lambda_max, q_bar;
	int k, i;
	slep_matrix *ATY, *AX;

	k = Y->n;
	assert( Y->m == A->m);
	assert( z>=0.0);
	sll_opts(opts);
	if( hasGot(opts, SlepOPT_q))
		(q=2.0 , opts->q = 2.0);
	else{
		q = opts->q;
		assert( q>=1.0);
	}

	initNormalization(A,opts);

	ATY = slepAllocDenseMatrix(X->m, X->n);
	AX = slepAllocDenseMatrix(Y->m, Y->n);

	// ATy
	slep_dgemm(SlepTranspose,SlepNoTranspose,1.0,A,Y,0.0,ATY);

	if( opts->rFlag == 0)
		lambda = z;
	else{
		slep_vector ATY__i;
		double temp;
		assert ( z>=0 && z<=1.0);
		if(q==1)
			q_bar = 1e30;
		else if( q> 1e6)
			q_bar = 1.0;
		else
			q_bar = q/(q-1.0);
		lambda_max = 0.0;
		ATY__i.dim = ATY->n;
		ATY__i.sparsity = SlepDense;
		ATY__i.inc = ATY->ld;
		for(i=0; i<ATY->m; i++){
			ATY__i.data.vctr = &(ATY->data.mtrx[i]);
			temp = slep_dpnrm(&ATY__i,q_bar);
			lambda_max = SLEPMAX(lambda_max,temp);
		}
		lambda = z * lambda_max;
	}

	if( opts->init == 2)
		slep_zerom(X);
	else{
		if( hasGot(opts,SlepOPT_X0)){
			slep_dmcopy(opts->X0,X);
			assert( opts->X0->m == A->n && opts->X0->n==k);
		}else
			slep_dmcopy(ATY,X);
	}
	// TODO Changed it to NORMALIZEDMM
	slep_dgemm(SlepNoTranspose,SlepNoTranspose,1.0,A,X,0.0,AX);

	if(opts->init == 0){
		// TODO ratio multiply
	}

	/* Armijo Goldstein line search scheme */
	if( opts->mFlag == 0 && opts->lFlag==0){
		int bFlag = 0, iterStep;
		double L = 1.0;
		double alphap = 0.0 , alpha = 1.0, beta;
		double l_sum , r_sum;
		slep_matrix *Xp, *XXp, *AXp, *S;
		slep_matrix *AS, *ATAS, *G, *V, *AV, *AXY;

		Xp = slepCopyToNewMatrix(X);
		AXp = slepCopyToNewMatrix(AX);
		XXp = slepAllocDenseMatrix(Xp->m, Xp->n);
		S = slepAllocDenseMatrix(Xp->m, Xp->n);
		AS = slepAllocDenseMatrix(AXp->m, AXp->n);
		ATAS = slepAllocDenseMatrix(Xp->m, Xp->n);
		G = slepAllocDenseMatrix(Xp->m, Xp->n);
		V = slepAllocDenseMatrix(Xp->m, Xp->n);
		AV = slepAllocDenseMatrix(AXp->m, AXp->n);
		AXY = slepAllocDenseMatrix(AXp->m, AXp->n);
		slep_zerom(XXp);

		for( iterStep = 0; iterStep < opts->maxIter; iterStep ++ ){
			beta = (alphap - 1.0) / alpha;
			slep_dmcopy(X,S);
			slep_daXpbY(SlepNoTranspose, beta, XXp,1.0,S);
			
			slep_dmcopy(AX,AS);
			slep_daXpbY(SlepNoTranspose, -beta, AXp, 1.0+beta, AS);

			slep_dgemm(SlepTranspose,SlepNoTranspose, 1.0,A,AS,0.0,ATAS);

			slep_dmcopy(ATAS, G);
			slep_daXpbY(SlepNoTranspose, -1.0, ATY, 1.0, G);

			slep_dmcopy(X,Xp);
			slep_dmcopy(AX,AXp);
			for(;;){
				slep_dmcopy(S,V);
				slep_daXpbY(SlepNoTranspose, -1.0/L, G, 1.0, V);
				eppMatrix(X->data.mtrx, V->data.mtrx, X->m, X->n, lambda/ L, q);
				slep_dmcopy(X, V);
				slep_daXpbY(SlepNoTranspose, -1.0, S, 1.0, V);

				slep_dgemm(SlepNoTranspose, SlepNoTranspose, 1.0, A, X, 0.0 , AX);
				slep_dmcopy(AX, AV);
				slep_daXpbY(SlepNoTranspose, -1.0, AS, 1.0, AV);

				r_sum = slep_dfrbnrm(V); r_sum = r_sum * r_sum;
				l_sum = slep_dfrbnrm(AV); l_sum = l_sum * l_sum;

				if( r_sum <= 1e-20){
					bFlag = 1;
					break;
				}
				if( l_sum <= r_sum * L)
					break;
				else
					L = SLEPMAX(2*L , l_sum/r_sum);
			}
			valueL->data.vctr[iterStep] = L;
			alphap = alpha;
			alpha = ( 1.0 + sqrt(4.0 * alpha * alpha + 1.0)) / 2.0;
			slep_dmcopy(X,XXp);
			slep_daXpbY(SlepNoTranspose, -1.0, Xp, 1.0, XXp);
			slep_dmcopy(AX, AXY);
			slep_daXpbY(SlepNoTranspose, -1.0, Y, 1.0 , AXY);
			funVal->data.vctr[iterStep] = slep_dfrbnrm(AXY);
			funVal->data.vctr[iterStep] = funVal->data.vctr[iterStep] * funVal->data.vctr[iterStep] / 2.0;
			{
				slep_vector X_i;
				double temp;
				temp=0.0; X_i.dim = X->n; X_i.inc = X->ld; X_i.sparsity = SlepDense;
				for(i=0; i<X->m; i++){
					X_i.data.vctr = &(X->data.mtrx[i]);
					temp += slep_dpnrm(&X_i,q);
				}
				funVal->data.vctr[iterStep] += lambda * temp;
			}
			if(bFlag)
				break;
			if( opts->tFlag == 0){
				if( iterStep >= 2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if(slep_dfrbnrm(XXp) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( slep_dfrbnrm(XXp) <= (opts->tol) * max(slep_dfrbnrm(Xp),1))
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeMatrix(Xp);
		slepFreeMatrix(XXp);
		slepFreeMatrix(AXp);
		slepFreeMatrix(S);

		slepFreeMatrix(AS);
		slepFreeMatrix(ATAS);
		slepFreeMatrix(G);
		slepFreeMatrix(V);
		slepFreeMatrix(AV);
		slepFreeMatrix(AXY);

		funVal->dim = iterStep;
		valueL->dim = iterStep;

	}

	/* Nemirovski's line search */
	if( opts->mFlag == 1 && opts->lFlag==0 && SLEPDoubleApproxEquals(opts->q,2.0) ){
		int bFlag = 0, iterStep;
		double L = 1.0;
		double alphap, alpha, beta;
		double l_sum , r_sum;
		slep_matrix *Xp, *XXp, *AXp, *S;
		slep_matrix *AS, *ATAS, *G, *V, *AV, *AXY;

		slep_matrix *U;
		slep_vector *t, *tp, *s_t, *v;

		XXp = slepAllocDenseMatrix(X->m, X->n);
		S = slepAllocDenseMatrix(X->m, X->n);
		AS = slepAllocDenseMatrix(AX->m, AX->n);
		ATAS = slepAllocDenseMatrix(X->m, X->n);
		G = slepAllocDenseMatrix(X->m, X->n);
		V = slepAllocDenseMatrix(X->m, X->n);
		AV = slepAllocDenseMatrix(AX->m, AX->n);
		AXY = slepAllocDenseMatrix(AX->m, AX->n);

		t = slepAllocDenseVector(X->m);
		tp = slepAllocDenseVector(X->m);
		s_t = slepAllocDenseVector(X->m);
		v = slepAllocDenseVector(X->m);
		U = slepAllocDenseMatrix(X->m, X->n);

		Xp = slepCopyToNewMatrix(X);
		AXp = slepCopyToNewMatrix(AX);
		slep_zerom(XXp);
		alphap = 0.0;
		alpha = 1.0;

		{
			slep_vector x__i=*t;
			x__i.dim = X->n;
			x__i.inc = X->ld;
			for(i=0; i< X->m; ++i){
				x__i.data.vctr = &(X->data.mtrx[i]);
				tp->data.vctr[i] = t->data.vctr[i] = slep_dnrm2(&x__i);
			}
		}

		for( iterStep = 0; iterStep < opts->maxIter; iterStep ++ ){
			beta = (alphap - 1.0) / alpha;

			slep_dmcopy(X,S);
			slep_daXpbY(SlepNoTranspose, beta, XXp,1.0,S);
			slep_dvcopy(t,s_t);
			slep_daxpby(-beta, tp, 1.0+beta, s_t);
			
			slep_dmcopy(AX,AS);
			slep_daXpbY(SlepNoTranspose, -beta, AXp, 1.0+beta, AS);

			// ATAS
			slep_dgemm(SlepTranspose,SlepNoTranspose, 1.0,A,AS,0.0,ATAS);

			slep_dmcopy(ATAS, G);
			slep_daXpbY(SlepNoTranspose, -1.0, ATY, 1.0, G);

			slep_dmcopy(X,Xp);
			slep_dmcopy(AX,AXp);
			slep_dvcopy(t,tp);
			for(;;){
				slep_dmcopy(S,U);
				slep_daXpbY(SlepNoTranspose, -1.0/L, G, 1.0, U);
				DMEMSET(v->data.vctr, lambda/L, v->dim, i);
				slep_daxpby(1.0,s_t, -1.0, v);

				ep21R(X->data.mtrx, t->data.vctr, U->data.mtrx, v->data.vctr, U->m, U->n);

				slep_dmcopy(X, V);
				slep_daXpbY(SlepNoTranspose, -1.0, S, 1.0, V);

				slep_dgemm(SlepNoTranspose, SlepNoTranspose, 1.0, A, X, 0.0 , AX);
				slep_dmcopy(AX, AV);
				slep_daXpbY(SlepNoTranspose, -1.0, AS, 1.0, AV);

				r_sum = slep_dfrbnrm(V); r_sum = r_sum * r_sum;
				r_sum = slep_ddot(t,t)+slep_ddot(s_t,s_t)-2.0*slep_ddot(t,s_t);
				l_sum = slep_dfrbnrm(AV); l_sum = l_sum * l_sum;

				if( r_sum <= 1e-20){
					bFlag = 1;
					break;
				}
				if( l_sum <= r_sum * L)
					break;
				else
					L = SLEPMAX(2*L , l_sum/r_sum);
			}
			valueL->data.vctr[iterStep] = L;
			alphap = alpha;
			alpha = ( 1.0 + sqrt(4.0 * alpha * alpha + 1.0)) / 2.0;
			slep_dmcopy(X,XXp);
			slep_daXpbY(SlepNoTranspose, -1.0, Xp, 1.0, XXp);
			slep_dmcopy(AX, AXY);
			slep_daXpbY(SlepNoTranspose, -1.0, Y, 1.0 , AXY);
			funVal->data.vctr[iterStep] = slep_dfrbnrm(AXY);
			funVal->data.vctr[iterStep] = funVal->data.vctr[iterStep] * funVal->data.vctr[iterStep] / 2.0;
			funVal->data.vctr[iterStep] += lambda * slep_dsum(t);
			if(bFlag)
				break;
			if( opts->tFlag == 0){
				if( iterStep >= 2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if(slep_dfrbnrm(XXp) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( slep_dfrbnrm(XXp) <= (opts->tol) * max(slep_dfrbnrm(Xp),1))
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeMatrix(Xp);
		slepFreeMatrix(XXp);
		slepFreeMatrix(AXp);
		slepFreeMatrix(S);

		slepFreeMatrix(AS);
		slepFreeMatrix(ATAS);
		slepFreeMatrix(G);
		slepFreeMatrix(V);
		slepFreeMatrix(AV);
		slepFreeMatrix(AXY);

		slepFreeMatrix(U);
		slepFreeVector(t);
		slepFreeVector(tp);
		slepFreeVector(s_t);
		slepFreeVector(v);
		funVal->dim = iterStep;
		valueL->dim = iterStep;

	}

	/* Adaptive line search */
	if( opts->mFlag == 1 && opts->lFlag==1 && SLEPDoubleApproxEquals(opts->q,2.0) ){
		int bFlag = 0, iterStep;
		double L = 1.0;
		double l_sum , r_sum;
		double gamma, alpha, alphap, beta;
		slep_matrix *Xp, *XXp, *AXp, *S;
		slep_matrix *AS, *ATAS, *G, *V, *AV, *AXY;

		slep_matrix *U;
		slep_vector *t, *tp, *s_t, *v;

		slep_matrix *ATAX, *ATAXp;
		slep_matrix *Xnew, *AXnew;
		slep_vector *v_t, *tnew;
		double tao;

		gamma = 1.0;

		Xp = slepCopyToNewMatrix(X);
		AXp = slepCopyToNewMatrix(AX);
		XXp = slepAllocDenseMatrix(Xp->m, Xp->n);
		S = slepAllocDenseMatrix(Xp->m, Xp->n);
		AS = slepAllocDenseMatrix(AXp->m, AXp->n);
		ATAS = slepAllocDenseMatrix(Xp->m, Xp->n);
		G = slepAllocDenseMatrix(Xp->m, Xp->n);
		V = slepAllocDenseMatrix(Xp->m, Xp->n);
		AV = slepAllocDenseMatrix(AXp->m, AXp->n);
		AXY = slepAllocDenseMatrix(AXp->m, AXp->n);

		t = slepAllocDenseVector(X->m);
		tp = slepAllocDenseVector(X->m);
		s_t = slepAllocDenseVector(X->m);
		v = slepAllocDenseVector(X->m);
		U = slepAllocDenseMatrix(Xp->m, Xp->n);

		ATAX = slepAllocDenseMatrix(Xp->m, Xp->n);
		ATAXp = slepAllocDenseMatrix(Xp->m, Xp->n);
		Xnew = slepAllocDenseMatrix(Xp->m, Xp->n); 
		AXnew = slepAllocDenseMatrix(AXp->m, AXp->n);
		v_t = slepAllocDenseVector(X->m);
		tnew = slepAllocDenseVector(X->m);
		{
			slep_vector x__i=*t;
			x__i.dim = X->n;
			x__i.inc = X->ld;
			for(i=0; i< X->m; ++i){
				x__i.data.vctr = &(X->data.mtrx[i]);
				tp->data.vctr[i] = t->data.vctr[i] = slep_dnrm2(&x__i);
			}
		}
		slep_zerom(XXp);
		//ATAX
		slep_dgemm(SlepTranspose, SlepNoTranspose, 1.0, A, AX, 0.0, ATAX);

		for( iterStep = 0; iterStep < opts->maxIter; iterStep ++ ){
			slep_dmcopy(ATAX, ATAXp);

			if(iterStep != 0)
				slep_dgemm(SlepTranspose, SlepNoTranspose, 1.0, A, AX, 0.0, ATAX);

			for(;;){
				if(iterStep != 0){
					alpha = ( -gamma + sqrt(gamma*gamma + 4.0*L*gamma))/(2.0*L);
					beta = (gamma - gamma * alphap) / (alphap * gamma + alphap * L * alpha);

					slep_dmcopy(X,S);
					slep_daXpbY(SlepNoTranspose, beta, XXp, 1.0, S);

					slep_dvcopy(t,s_t);
					slep_daxpby(-beta, tp, 1.0+beta, s_t);
					slep_dmcopy(AX, AS);
					slep_daXpbY(SlepNoTranspose,-beta, AXp,1.0+beta, AS);

					slep_dmcopy(ATAX, ATAS);
					slep_daXpbY(SlepNoTranspose,-beta, ATAXp,1.0+beta, ATAS);
				}else{
					alpha = (-1.0 + sqrt(5.0))/2.0;
					beta = 0.0;
					slep_dmcopy(X,S);
					slep_dvcopy(t,s_t);
					slep_dmcopy(AX,AS);
					slep_dmcopy(ATAX,ATAS);
				}
				slep_dmcopy(ATAS, G);
				slep_daXpbY(SlepNoTranspose, -1.0, ATY, 1.0, G);

				slep_dmcopy(S,U);
				slep_daXpbY(SlepNoTranspose, -1.0/L, G, 1.0, U);

				DMEMSET(v->data.vctr, lambda/L, v->dim, i);
				slep_daxpby(1.0,s_t, -1.0, v);

				ep21R(Xnew->data.mtrx, tnew->data.vctr, U->data.mtrx, v->data.vctr, U->m, U->n);

				slep_dmcopy(Xnew, V);
				slep_daXpbY(SlepNoTranspose, -1.0, S, 1.0, V);

				slep_dvcopy(tnew, v_t);
				slep_daxpby(-1.0, s_t, 1.0, v_t);

				slep_dgemm(SlepNoTranspose, SlepNoTranspose, 1.0, A, Xnew, 0.0 , AXnew);

				slep_dmcopy(AXnew, AV);
				slep_daXpbY(SlepNoTranspose, -1.0, AS, 1.0, AV);

				r_sum = slep_dfrbnrm(V); r_sum = r_sum * r_sum;
				r_sum += slep_ddot(v_t,v_t);
				l_sum = slep_dfrbnrm(AV); l_sum = l_sum * l_sum;

				if( r_sum <= 1e-20){
					bFlag = 1;
					break;
				}
				if( l_sum <= r_sum * L)
					break;
				else
					L = SLEPMAX(2*L , l_sum/r_sum);
			}

			gamma = L * alpha * alpha;
			alphap = alpha;

			valueL->data.vctr[iterStep] = L;
			tao = L * r_sum / l_sum ;
			if(tao >= 5.0)
				L = L * 0.8;

			slep_dmcopy(X, Xp);
			slep_dmcopy(Xnew, X);
			slep_dmcopy(X,XXp);
			slep_daXpbY(SlepNoTranspose, -1.0, Xp, 1.0, XXp);
			slep_dmcopy(AX, AXp);
			slep_dmcopy(AXnew, AX);
			slep_dvcopy(t,tp);
			slep_dvcopy(tnew,t);

			slep_dmcopy(AX, AXY);
			slep_daXpbY(SlepNoTranspose, -1.0, Y, 1.0 , AXY);

			funVal->data.vctr[iterStep] = slep_dfrbnrm(AXY);
			funVal->data.vctr[iterStep] = funVal->data.vctr[iterStep] * funVal->data.vctr[iterStep] / 2.0;
			funVal->data.vctr[iterStep] += lambda * slep_dsum(t);
			if(bFlag)
				break;
			if( opts->tFlag == 0){
				if( iterStep >= 2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < opts->tol)
						break;
				}
			}else if( opts->tFlag == 1){
				if(iterStep>=2){
					if( fabs(funVal->data.vctr[iterStep] - funVal->data.vctr[iterStep-1]) < (opts->tol)*(funVal->data.vctr[iterStep-1]) )
						break;
				}
			}else if(opts->tFlag == 2){
				if( funVal->data.vctr[iterStep] <= opts->tol)
					break;
			}else if(opts->tFlag == 3){
				if(slep_dfrbnrm(XXp) <= opts->tol)
					break;
			}else if(opts->tFlag == 4){
				if( slep_dfrbnrm(XXp) <= (opts->tol) * max(slep_dfrbnrm(Xp),1))
					break;
			}else if(opts->tFlag == 5){
				if( iterStep >= opts->maxIter)
					break;
			}
		}
		slepFreeMatrix(Xp);
		slepFreeMatrix(XXp);
		slepFreeMatrix(AXp);
		slepFreeMatrix(S);

		slepFreeMatrix(AS);
		slepFreeMatrix(ATAS);
		slepFreeMatrix(G);
		slepFreeMatrix(V);
		slepFreeMatrix(AV);
		slepFreeMatrix(AXY);

		slepFreeMatrix(U);
		slepFreeVector(t);
		slepFreeVector(tp);
		slepFreeVector(s_t);
		slepFreeVector(v);

		slepFreeMatrix(ATAX);
		slepFreeMatrix(ATAXp);
		slepFreeMatrix(Xnew);
		slepFreeMatrix(AXnew);
		slepFreeVector(v_t);
		funVal->dim = iterStep;
		valueL->dim = iterStep;

	}
	slepFreeMatrix(ATY);
	slepFreeMatrix(AX);
}